#pragma config(Sensor, in1,    ProgramPotentio,     sensorPotentiometer)
#pragma config(Sensor, in2,    LineLeft,            sensorLineFollower)
#pragma config(Sensor, in3,    LineMiddle,          sensorLineFollower)
#pragma config(Sensor, in4,    LineRight,           sensorLineFollower)
#pragma config(Sensor, in5,    ProgramPotentio2,    sensorPotentiometer)
#pragma config(Sensor, in6,    LoneLine,            sensorLineFollower)
#pragma config(Sensor, dgtl1,  ArmEncoder,          sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  LeftEncoder,         sensorQuadEncoder)
#pragma config(Sensor, dgtl5,  RightEncoder,        sensorQuadEncoder)
#pragma config(Sensor, dgtl7,  Ultrasonic,          sensorSONAR_mm)
#pragma config(Sensor, dgtl9,  TubeLimit,           sensorTouch)
#pragma config(Sensor, dgtl11, CloseLimit,          sensorTouch)
#pragma config(Sensor, dgtl12, OpenLimit,           sensorTouch)
#pragma config(Motor,  port1,           LeftWheel1,    tmotorNormal, openLoop)
#pragma config(Motor,  port2,           LeftWheel2,    tmotorNormal, openLoop)
#pragma config(Motor,  port3,           LeftArm,       tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port4,           LeftArm2,      tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port5,           ClawLeft,      tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port6,           ClawRight,     tmotorNormal, openLoop)
#pragma config(Motor,  port7,           RightArm2,     tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port8,           RightArm,      tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port9,           RightWheel2,   tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port10,          RightWheel1,   tmotorNormal, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#include "Vex_Competition_Includes_modified.c"   //Main competition background code...do not modify!

// Global Variables/functions:
const bool debugMode = false;
const int controlSchemeNumber = 1;
const int armDownPower = 23;
const int armUpPower = 120;
const int clawOpenPower = 120;
const int clawClosePower = 100;
const int clawHoldPower = 65;
const int maxMovePower = 110;
const int maxDrivePower = 80;
const int bookDrivePower = 105;
const int minMovePower = 25;
const int wheelBrakePower = 11;
const int armEncode[11] = {0, 85, 290, 380, 370, 370, 130, 170, 230, 95, 95}; //Arm Encode Heights
const int armPower[11] = {0, 37, 53, 40, 35, 0, 25, 35, 33, 27, 0}; // Min arm powers at each height
int bob = 1; // debug variable

// Threshold variables
const int joystickThreshold = 15;
const int bookThreshold = 120;
//int accelThreshold = 15;
const int armEncodeThreshold = 5;
const int driveThreshold = 25;

// Recyclable variables
int integral = 0; int armTimer = 0;

// Utility variable (changing them directly causes things to happen - make sure debugMode is set to 1)
int armLeftPower = 0; int armRightPower = 0; int wheelLeftPower = 0; int wheelRightPower = 0; int clawPower = 0;
/*int accx = 0; int prevx = 0;*/
int wiggledir = -1;
bool wiggleon = false;

/*------------------------Button Press Detection---------------------*/

int btn7u_isPressed = 0;
int btn7l_isPressed = 0;
int btn7r_isPressed = 0;
int btn7d_isPressed = 0;
int btn8u_isPressed = 0;
int btn8l_isPressed = 0;
int btn8r_isPressed = 0;
int btn8d_isPressed = 0;
int btn5u_isPressed = 0;
int btn5d_isPressed = 0;
int btn6u_isPressed = 0;

/*-------------------------State Variables---------------------*/

// Function activation
int clawState = 3; // 1=closed, 2=closing, 3=neutral/human control, 4=opening, 5=opened
int armState = 6; // 1=bottom, 2=weight level, 3=above weight/high level, 4=above high, 6=user control/default, 7=1tubeslow, 8=2tubeslow
int driveState = 3; // 1-Computer, 2=Turn, 3=User Control/Default
//int reflexMode = 0; // 1 - activates reflex mode
bool calibrateArm_inProgress = false;

// Progress detection
int oldClawState;
int armAction = 0; // 1=in motion 0=stopped
int gameState = 0; //  0=Start, 1=Autonomous, 2=User Control
int tubeClicked = 0;

/*-----------------------User Functions Includes (DON'T TOUCH)--------------------*/

// Links external files into this one (find them in the folder)

#include "management.c" // Management functions
#include "autofunctions.c" // Autonomous functions
#include "usercontrol.c" // User Control functions
#include "autonomous.c" //  Autonomous code

/*-------------------------------Game Functions---------------------------*/

// Pre-autonomous functions:

void pre_auton()
{
  // All activities that occur before the competition starts
  // Example: clearing encoders, setting servo positions, ...
  SensorValue[ArmEncoder] = 0;
  SensorValue[LeftEncoder]= 0;
  SensorValue[RightEncoder] = 0;
}

// Robot Management Tasks:
task claw() // task to manage claw
{
  while (true)
  {
    updateClawState();
    if (SensorValue[TubeLimit] == 1) {
      tubeClicked = 1;
    }
    else {
      tubeClicked = 0;
    }
    motor[ClawLeft] = clawPower; // set the power of the claw to whatever the value of the variable clawPower is
    motor[ClawRight] = clawPower;
  }
}
task arm() //task to manage arm
{
  while (true)
  {
    if (calibrateArm_inProgress) {
      calibrateArm();
    }
    else {
      manageArm();
    }
    if (debugMode) {
      armLeftPower = bob;
      armRightPower = bob;
    }
    motor[LeftArm] = armLeftPower; // sets arm power to whatever the variable armLeftPower and armRightPower is
    motor[LeftArm2] = armLeftPower;
    motor[RightArm2] = armRightPower;
    motor[RightArm] = armRightPower;
    if (SensorValue[in7] < 100) { // Resets arm encoder when arm hits bottom
      SensorValue[ArmEncoder] = 0;
    }
    wait1Msec(10);
  }
}
task drive() //task to manage drive
{
  while (true)
  {
    if (wiggleon) { // Wiggles if wigglemode is on
      wiggle();
    }
    motor[LeftWheel1] = wheelLeftPower;
    motor[LeftWheel2] = wheelLeftPower;
    motor[RightWheel1] = wheelRightPower;
    motor[RightWheel2] = wheelRightPower;
  }
}

// Autonomous:
task autonomous()
{
  wait1Msec(10);
  gameState = 1;
  SensorValue[ArmEncoder] = 0;
  SensorValue[LeftEncoder] = 0;
  SensorValue[RightEncoder] = 0;
  if (SensorValue[ProgramPotentio] >= 3000) { //Picks autonomous depending on pot position (commented out ones not used)
    // Blue autonomous
    if (SensorValue[ProgramPotentio2] >= 3000) {
      autonomousOne(); //11 - Score one then dump in ladder
    }
    else if (SensorValue[ProgramPotentio2] >= 2000 && SensorValue[ProgramPotentio2] < 3000) {
      autonomousTwo(); //12 - DT auto
    }
	  else if (SensorValue[ProgramPotentio2] >= 1000 && SensorValue[ProgramPotentio2] < 2000) {
	    autonomousThree(); //13 - Dump in ladder, descore, dump
	  }
	  /*else if (SensorValue[ProgramPotentio2] < 1000) {
	    autonomousFour(); //14 - Dump in ladder, score high
	  }*/
  }
  else if (SensorValue[ProgramPotentio] >= 2000 && SensorValue[ProgramPotentio] < 3000) {
    if (SensorValue[ProgramPotentio2] >= 3000) {
      autonomousFive(); //21 - Descore, score
    }
    /*else if (SensorValue[ProgramPotentio2] >= 2000 && SensorValue[ProgramPotentio2] < 3000) {
      autonomousSix(); //22 - Back area: dump, score
    }
    // Red autonomous
	  else if (SensorValue[ProgramPotentio2] >= 1000 && SensorValue[ProgramPotentio2] < 2000) {
	    autonomousSeven(); //23 - Red version of 11
	  }
	  else if (SensorValue[ProgramPotentio2] < 1000) {
	    autonomousEight(); //24 - Red version of 12
	  }*/
  }
  else if (SensorValue[ProgramPotentio] >= 1000 && SensorValue[ProgramPotentio] < 2000) {
    /*if (SensorValue[ProgramPotentio2] >= 3000) {
      autonomousNine(); //31 - Red version of 13
    }
    else if (SensorValue[ProgramPotentio2] >= 2000 && SensorValue[ProgramPotentio2] < 3000) {
      autonomousTen(); //32 - Red version of 14
    }*/
	  else if (SensorValue[ProgramPotentio2] >= 1000 && SensorValue[ProgramPotentio2] < 2000) {
	    autonomousEleven(); //33 - Red version of 21
	  }
	  /*else if (SensorValue[ProgramPotentio2] < 1000) {
	    autonomousTwelve(); //34 - Red version of 22
	  }*/
  }
  else if (SensorValue[ProgramPotentio] < 1000) {
    // Other
    /*if (SensorValue[ProgramPotentio2] >= 3000) {
      autonomousThirteen(); //41 - Failsafe 1: flip, grab, move
    }
    else if (SensorValue[ProgramPotentio2] >= 2000 && SensorValue[ProgramPotentio2] < 3000) {
      autonomousFourteen(); //42 - Failsafe 2: flip, grab, ram
    }*/
	  else if (SensorValue[ProgramPotentio2] >= 1000 && SensorValue[ProgramPotentio2] < 2000) {
	    autonomousFifteen(); //43 - Failsafe 3: RAM - reverse, claw flip at end
	  }
	  else if (SensorValue[ProgramPotentio2] < 1000) {
	    programChallenge(); //44 - Program Challenge
	  }
  }
}

// User Control:
task usercontrol()
{
  wait1Msec(10);
  gameState = 2;
  driveState = 3;
  //reflexMode = 0;

  // "Smart" starting arm state picker
  if (SensorValue[ArmEncoder] < 25) {
    armState = 1;
  }
  else if (SensorValue[ArmEncoder] >= 25 && SensorValue[ArmEncoder] < 120) {
    armState = 2;
  }
  else {
    armState = 4;
  }

  SensorValue[LeftEncoder] = 0;
  SensorValue[RightEncoder] = 0;

  while (true)
  {
    if (vexRT[Btn7U] == 1 && btn7u_isPressed == 0) // Arm calibration
    {
      armTimer = 0;
      calibrateArm_inProgress = true;
    }
    if (vexRT[Btn7L] == 1 && btn7l_isPressed == 0) // Arm to one tube above low base
    {
      armState = 7;
    }
    if (vexRT[Btn7R] == 1 && btn7r_isPressed == 0) // Arm to two tubes above low base
    {
      armState = 8;
    }
    if (vexRT[Btn7D] == 1 && btn7d_isPressed == 0) // Arm to high goal
    {
      armState = 9;
    }
    if (vexRT[Btn8U] == 1 && btn8u_isPressed == 0) // Not used
    {

    }
    if (vexRT[Btn8L] == 1 && btn8l_isPressed == 0) // Arm on ground
    {
      if (armState < 6) {
        armState = 6;
        armLeftPower = 0;
        armRightPower = 0;
      }
      else if (armState == 6) {
        armState = 3;
      }
    }
    else if (vexRT[Btn8R] == 1 && btn8r_isPressed == 0) // Arm at above high height
    {
      moveForwardTP(-450);
    }
    if (vexRT[Btn8D] == 1 && btn8d_isPressed == 0) // Arm manual/auto switch
    {
      armState = 10;
    }
    // Controls
    if (SensorValue[in8] < 100) {
      clawAutoClose();
    }
    useControls(controlSchemeNumber);
    //Check Buttons
    btn7u_isPressed = vexRT[Btn7U];
    btn7l_isPressed = vexRT[Btn7L];
    btn7r_isPressed = vexRT[Btn7R];
    btn7d_isPressed = vexRT[Btn7D];
    btn8u_isPressed = vexRT[Btn8U];
    btn8l_isPressed = vexRT[Btn8L];
    btn8r_isPressed = vexRT[Btn8R];
    btn8d_isPressed = vexRT[Btn8D];
    btn5u_isPressed = vexRT[Btn5U];
    btn5d_isPressed = vexRT[Btn5D];
    btn6u_isPressed = vexRT[Btn6U];
    wait1Msec(10);
  }
}
